﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Lipesin.MP710
{
	/// <summary>
	/// Драйвер шагового мотора
	/// </summary>
	public class SMDriver
	{
		private MP710Driver _mp710;
		private byte[] _ports;
		private int _winding = 0;
		private bool _directionForward = true;

		/// <summary>
		/// Номер шага. Значение меняется при каждом шаге двигателя
		/// </summary>
		public int Step = 0;

		#region constructor
		/// <summary>
		/// 
		/// </summary>
		/// <param name="mp710"></param>
		/// <param name="ports">
		/// Список портов, подключённых к обмоткам.
		/// например new byte[] { 0, 1, 2, 3 } для четырёх первых портов
		/// </param>
		public SMDriver(MP710Driver mp710, byte[] ports)
		{
			_mp710 = mp710;
			_ports = ports;
			Reset();
		}
		#endregion constructor

		/// <summary>
		/// Сбрасывает питание со всех обмоток
		/// </summary>
		public void Reset()
		{
			foreach (byte p in _ports)
				_mp710.SetPort(p, false);
		}
		/// <summary>
		/// Направление вращения
		/// </summary>
		public bool DirectionForward
		{
			get { return _directionForward; }
			set { _directionForward = value; }
		}
		public void DoStep()
		{
			DoStepInternal();
		}
		private void DoStepInternal(long halfStepTiks = 0)
		{
			int windingOld = _winding;
			if (_directionForward)
			{
				_winding++;
				if (_winding >= _ports.Length) _winding = 0;
			}
			else
			{
				_winding--;
				if (_winding < 0) _winding = _ports.Length - 1;
			}
			_mp710.SetPort(_ports[_winding], true);
			if (halfStepTiks > 0) System.Threading.SpinWait.SpinUntil(() => halfStepTiks < DateTime.Now.Ticks, 1000);
			_mp710.SetPort(_ports[windingOld], false);
		}
		public int DoSteps(int amount, double stepsPerSecond, double acceleration)
		{
			const long TicksInSecond = 1000 * 10000;

			acceleration = 0;//тест версионности, теперь с работы
			int accSteps = 0;//шагов для разгона
			int[] stepTimings;
			if (acceleration > 0)//высчитываем стадии разгона/торможения
			{
				accSteps = Convert.ToInt32(Math.Truncate(Math.Pow(stepsPerSecond, 2) / (2 * acceleration)));//шагов для разгона
				if (accSteps * 2 > amount) accSteps = amount / 2;//если полноценно разогнаться не успеваем
				stepTimings = new int[accSteps * 2];
				for (int step = 0; step < accSteps; step++)
				{
					stepTimings[step * 2] = Convert.ToInt32(Math.Sqrt(2 * (step + 0.5) / acceleration) * TicksInSecond);//полушаг
					stepTimings[step * 2 + 1] = Convert.ToInt32(Math.Sqrt(2 * (step + 1) / acceleration) * TicksInSecond);//шаг
				}
				for (int step = accSteps; step > 0; step--)//из абсолютных таймингов в относительные
				{
					stepTimings[step * 2] -= stepTimings[(step - 1) * 2];
					stepTimings[step * 2 + 1] -= stepTimings[(step - 1) * 2 + 1];
				}
			}
			long baseDelay = Convert.ToInt64(TicksInSecond / stepsPerSecond);
			long halfDelay = baseDelay / 2;
			long nextHalfStep = 0;//Точка висения на двух обмотках в середине текущего шага
			long nextStep = 0;//Точка окончания текущего шага
			long nextOverStep = 0;//Точка потери ритма (отставание более, чем на пол шага)
			int errors = -1;
			for (int count = 0; count < amount; count++)
			{
				if (DateTime.Now.Ticks > nextOverStep)
				{
					//Включение обмоток отстало от ритма. Реинициализируем ритм. так-же штатная инициализация при старте
					errors++;
					nextStep = DateTime.Now.Ticks + baseDelay;
					nextHalfStep = nextStep - halfDelay;
					nextOverStep = nextStep + halfDelay;
				}
				else
				{
					nextHalfStep = nextStep + halfDelay;
					nextStep += baseDelay;
					nextOverStep += baseDelay;
				}
				DoStepInternal(nextHalfStep);
				System.Threading.SpinWait.SpinUntil(() => nextStep < DateTime.Now.Ticks, 1000);
			}
            Reset();
			return errors;
		}
	}
}
